#!  /usr/bin/env python
# coding=utf-8

import rospy
# import socket
# import os
# import json

# from serial_demo.msg import ti5_polyJC_Lists
# from serial_demo.msg import ti5_motorStatus

import numpy as np
import math
"""
注：机械臂实际基座高175，三维模型高190，逆解按照模型计算
即若想实物机械臂达到(x,y,z)则给定的指令应是(x,y,z+15)
"""
L0 = 190
L2 = 400
L3 = 300
LT = 300

# MDH
alfa_i_1 = np.zeros((6,1),np.float)
a_i_1 = np.zeros((6,1),np.float)
theta_i = np.zeros((6,1),np.float)
d_i = np.zeros((6,1),np.float)

pi = math.pi

alfa_i_1[0]= 0 ;     a_i_1[0] = 0 ;    theta_i[0] = 0;       d_i[0] = 190

alfa_i_1[1]= pi/2;   a_i_1[1] = 0;     theta_i[1] = pi/2;    d_i[1] = 0

alfa_i_1[2]= 0;      a_i_1[2] = 400;   theta_i[2] = -pi/2;   d_i[2] = 0

alfa_i_1[3]= -pi/2;  a_i_1[3] = 0;     theta_i[3] = 0;       d_i[3] = 300

alfa_i_1[4]= pi/2;   a_i_1[4] = 0;     theta_i[4] = pi/2;    d_i[4] = 0

alfa_i_1[5]= pi/2;   a_i_1[5] = 0;     theta_i[5] = 0;       d_i[5] = 300



Tw_00 = np.eye(4, dtype = np.float)
Tw_00[2,3] = L0

T6_t = np.eye(4, dtype = np.float)
T6_t[2,3] = LT

R00_6 = np.eye(3,dtype= np.float)


class MDH_TF_f:
    @staticmethod
    def T_fram(alfa, a, theta, d):
        # type:(np.ndarray, np.ndarray, np.ndarray, np.ndarray)->np.ndarray
        T = np.zeros((6,4,4),dtype= np.float)
        for i in range(0,4):
            T[i,:,:] = np.eye(4, dtype=np.float)
            pass
        
        for i in range(0,6):
            T[i,0,0] = math.cos(theta[i])
            T[i,0,1] = -math.sin(theta[i])
            T[i,0,2] = 0.0
            T[i,0,3] = a[i]
            T[i,1,0] = math.sin(theta[i]) * math.cos(alfa[i])
            T[i,1,1] = math.cos(theta[i]) * math.cos(alfa[i])
            T[i,1,2] = -math.sin(alfa[i])
            T[i,1,3] = -math.sin(alfa[i]) * d[i]
            T[i,2,0] = math.sin(theta[i]) * math.sin(alfa[i])
            T[i,2,1] = math.cos(theta[i]) * math.sin(alfa[i])
            T[i,2,2] = math.cos(alfa[i])
            T[i,2,3] = math.cos(alfa[i]) * d[i]
            pass
        return T
        pass

    # 0 <= a < b <= n  Tf = array(n,4,4) has transfor of fram i-1 to i
    @staticmethod
    def T_fram_a_b(a,b,Tf):
        # type:(int, int, np.ndarray)->np.ndarray
        
        T = np.eye(4, dtype=np.float)
        for i in range(0,b-a):
            T = np.dot(T , Tf[a+i,:,:])
            pass

        return T
        pass
    
    @staticmethod
    def R_fram_0_4(t1,t2,t3):
        # type:(float,float,float)->np.ndarray
    
        R = np.eye(3,dtype= np.float)
        R[0,0] = math.cos(t1)*math.cos(t2)*math.cos(t3) - math.cos(t1)*math.sin(t2)*math.sin(t3)
        R[0,1] = -math.sin(t1)
        R[0,2] = -math.cos(t1)*math.cos(t2)*math.sin(t3) - math.cos(t1)*math.sin(t2)*math.cos(t3)
        R[1,0] = math.sin(t1)*math.cos(t2)*math.cos(t3) - math.sin(t1)*math.sin(t2)*math.sin(t3)
        R[1,1] = math.cos(t1)
        R[1,2] = -math.sin(t1)*math.cos(t2)*math.sin(t3) - math.sin(t1)*math.sin(t2)*math.cos(t3)
        R[2,0] = math.sin(t2)*math.cos(t3) + math.cos(t2)*math.sin(t3)
        R[2,1] = 0
        R[2,2] = -math.sin(t2)*math.sin(t3) + math.cos(t2)*math.cos(t3)


        return R
        pass

    @staticmethod
    def R_fram_0_4_inv(t1,t2,t3):
        # type:(float,float,float)->np.ndarray

        Rstar = np.eye(3,dtype= np.float) # R*
        Rstar[0,0] = math.cos(t1)*math.cos(t2+t3)
        Rstar[0,1] = math.sin(t1)*math.cos(t2+t3)
        Rstar[0,2] = math.sin(t2+t3)
        Rstar[1,0] = -math.sin(t1)
        Rstar[1,1] = math.cos(t1)
        Rstar[1,2] = 0
        Rstar[2,0] = -math.cos(t1)*math.sin(t2+t3) 
        Rstar[2,1] = -math.sin(t1)*math.sin(t2+t3)
        Rstar[2,2] = math.cos(t2+t3)

        return Rstar
        pass

    pass

def ti5_ik(cJdeg, Targ):
    # type:(np.ndarray,np.ndarray)->np.ndarray
    """ti5逆解脚本
        input:current joint
              target pose
        output: solution
    """
    theta = np.zeros((8,6),np.float)
    theta16 = np.zeros((16,6),np.float)
    px = Targ[0,3]-Targ[0,2]*LT
    py = Targ[1,3]-Targ[1,2]*LT
    pz = Targ[2,3]-Targ[2,2]*LT - L0

    R00_6[:] = Targ[0:3,0:3]
    
    extraSolution = 0

    # theta1 1 solution
    if abs(px) <= 0.0001 and abs(py) <= 0.0001: # x,y=0,0
        theta1_1 = 0
    else:
        theta1_1 = math.atan2(py,px)
        pass

    # theta1 2 solution
    if theta1_1>0:
        theta1_2 = theta1_1 - pi
    else:
        theta1_2 = theta1_1 + pi  
        pass
    
    # theta2,3 
    xx = math.sqrt(px*px + py*py)
    
    LL = px*px + py*py + pz*pz
    L = math.sqrt(LL)

    fa = math.atan2(abs(pz),xx)

    if L==L2+L3:
        # only one solution
        if pz>=0:
            theta2_1 = -pi/2+fa
            theta3_1 = 0
            theta2_2 = -pi/2+fa
            theta3_2 = 0     
        elif pz<0:
            theta2_1 = -pi/2-fa
            theta3_1 = 0
            theta2_2 = -pi/2-fa
            theta3_2 = 0      
    else:
        alfa = math.acos((LL + L2*L2 - L3*L3)/(2*L*L2))
        beta = math.acos((L2*L2 + L3*L3 - LL)/(2*L2*L3))
        if pz>=0:
            # 1 solution
            theta2_1 = alfa+fa-pi/2
            theta3_1 = beta-pi
            # 2 solution
            theta2_2 = -alfa+fa-pi/2
            theta3_2 = -beta+pi
        elif pz<0:
            # 1 solution
            theta2_1 = alfa-fa-pi/2
            theta3_1 = beta-pi
            # 2 solution
            theta2_2 = -alfa-fa-pi/2
            theta3_2 = -beta+pi
        pass

    # front 3 axes
    # 1
    theta[0,0] = theta1_1
    theta[0,1] = theta2_1
    theta[0,2] = theta3_1

    theta[4,0] = theta1_1
    theta[4,1] = theta2_1
    theta[4,2] = theta3_1

    # 2
    theta[1,0] = theta1_2
    theta[1,1] = -theta2_1
    theta[1,2] = -theta3_1

    theta[5,0] = theta1_2
    theta[5,1] = -theta2_1
    theta[5,2] = -theta3_1

    # 3
    theta[2,0] = theta1_1
    theta[2,1] = theta2_2
    theta[2,2] = theta3_2

    theta[6,0] = theta1_1
    theta[6,1] = theta2_2
    theta[6,2] = theta3_2

    # 4
    theta[3,0] = theta1_2
    theta[3,1] = -theta2_2
    theta[3,2] = -theta3_2

    theta[7,0] = theta1_2
    theta[7,1] = -theta2_2
    theta[7,2] = -theta3_2

    # theta4,5,6******************

    # solve last 3 axes by front 3 axes
    for j in range(0,4):
        th = np.zeros((3,1),np.float)
      
        th = theta[j,0:3] 
           
        R00_4_inv = MDH_TF_f.R_fram_0_4_inv(th[0],th[1],th[2])
        
        R4_6 = np.dot(R00_4_inv, R00_6)  # R4_6=(R00_4^-1)*R00_6
        R4_6_33 = R4_6[2,2]

        if (abs(R4_6_33-1)>0.0001) and ( abs(R4_6_33+1)>0.0001): # !=1 and !=-1
            beta = math.atan2(math.sqrt(R4_6[2,0]*R4_6[2,0] + R4_6[2,1]*R4_6[2,1]), R4_6_33)
            alfa = math.atan2(R4_6[1,2]/math.sin(beta), R4_6[0,2]/math.sin(beta))
            gama = math.atan2(R4_6[2,1]/math.sin(beta), -R4_6[2,0]/math.sin(beta))
        elif abs(R4_6_33-1)<=0.0001: # =1
            alfa = 0
            beta = 0
            gama = math.atan2(-R4_6[0,1],R4_6[0,0])
        elif abs(R4_6_33+1)<=0.0001: # =-1
            alfa = 0
            beta = pi
            gama = math.atan2(R4_6[0,1],-R4_6[0,0])
            pass

        # 1 solution
        if alfa<0:
            theta4_1 = alfa+pi
        else:
            theta4_1 = alfa-pi
            pass
        
        theta5_1 = beta+pi/2 # add axis 5 default extra offset
        theta6_1 = gama 

        # 2 solution
        theta5_2 = -beta+pi/2

        if theta4_1>=0:
            theta4_2 = theta4_1-pi
        else:
            theta4_2 = theta4_1+pi   
            pass

        if theta6_1>=0:
            theta6_2 = theta6_1-pi
        else:
            theta6_2 = theta6_1+pi   
            pass
        
        # each set of solution for the front 3 axes 
        # corresponds to 2 sets of solution for the last 3 axes
        theta[j,3] = theta4_1
        theta[j,4] = theta5_1
        theta[j,5] = theta6_1
        
        theta[j+4,3] = theta4_2
        theta[j+4,4] = theta5_2
        theta[j+4,5] = theta6_2

        if (pi-abs(theta6_1)) <= 0.001: # theta6_1= +-180
            extraSolution = extraSolution+1

            theta16[7+extraSolution,0] = theta[j,0]
            theta16[7+extraSolution,1] = theta[j,1]
            theta16[7+extraSolution,2] = theta[j,2]
            theta16[7+extraSolution,3] = theta[j,3]
            theta16[7+extraSolution,4] = theta[j,4]
            theta16[7+extraSolution,5] = -theta6_1
        elif abs(theta6_1) <= 0.001: # theta6_1=0 theat6_2= +-180
            extraSolution = extraSolution+1

            theta16[7+extraSolution,0] = theta[j+4,0]
            theta16[7+extraSolution,1] = theta[j+4,1]
            theta16[7+extraSolution,2] = theta[j+4,2]
            theta16[7+extraSolution,3] = theta[j+4,3]
            theta16[7+extraSolution,4] = theta[j+4,4]
            theta16[7+extraSolution,5] = -theta6_2
            
            pass

        pass # end of for j in range(0,4):
    theta16[0:8,:] = theta[0:8,:]
    if extraSolution < 8:
        theta16 = np.delete(theta16,obj=np.s_[8+extraSolution:16], axis=0) 
        pass
    # print(np.around(theta16*180/pi,3))
    return theta16
    pass

def ti5_ik_print(cJdeg, Targ):
    # type:(np.ndarray,np.ndarray)->np.ndarray
    """ti5逆解脚本
        input:current joint
              target pose
        output: solution
    """
    theta = np.zeros((8,6),np.float)
    theta16 = np.zeros((16,6),np.float)
    px = Targ[0,3]-Targ[0,2]*LT
    py = Targ[1,3]-Targ[1,2]*LT
    pz = Targ[2,3]-Targ[2,2]*LT - L0

    R00_6[:] = Targ[0:3,0:3]
    
    extraSolution = 0

    # theta1 1 solution
    if abs(px) <= 0.0001 and abs(py) <= 0.0001: # x,y=0,0
        theta1_1 = 0
    else:
        theta1_1 = math.atan2(py,px)
        pass

    # theta1 2 solution
    if theta1_1>0:
        theta1_2 = theta1_1 - pi
    else:
        theta1_2 = theta1_1 + pi  
        pass
    
    # theta2,3 
    xx = math.sqrt(px*px + py*py)
    
    LL = px*px + py*py + pz*pz
    L = math.sqrt(LL)

    fa = math.atan2(abs(pz),xx)

    if L==L2+L3:
        # only one solution
        if pz>=0:
            theta2_1 = -pi/2+fa
            theta3_1 = 0
            theta2_2 = -pi/2+fa
            theta3_2 = 0     
        elif pz<0:
            theta2_1 = -pi/2-fa
            theta3_1 = 0
            theta2_2 = -pi/2-fa
            theta3_2 = 0      
    else:
        alfa = math.acos((LL + L2*L2 - L3*L3)/(2*L*L2))
        beta = math.acos((L2*L2 + L3*L3 - LL)/(2*L2*L3))
        if pz>=0:
            # 1 solution
            theta2_1 = alfa+fa-pi/2
            theta3_1 = beta-pi
            # 2 solution
            theta2_2 = -alfa+fa-pi/2
            theta3_2 = -beta+pi
        elif pz<0:
            # 1 solution
            theta2_1 = alfa-fa-pi/2
            theta3_1 = beta-pi
            # 2 solution
            theta2_2 = -alfa-fa-pi/2
            theta3_2 = -beta+pi
        pass

    # front 3 axes
    # 1
    theta[0,0] = theta1_1
    theta[0,1] = theta2_1
    theta[0,2] = theta3_1

    theta[4,0] = theta1_1
    theta[4,1] = theta2_1
    theta[4,2] = theta3_1

    # 2
    theta[1,0] = theta1_2
    theta[1,1] = -theta2_1
    theta[1,2] = -theta3_1

    theta[5,0] = theta1_2
    theta[5,1] = -theta2_1
    theta[5,2] = -theta3_1

    # 3
    theta[2,0] = theta1_1
    theta[2,1] = theta2_2
    theta[2,2] = theta3_2

    theta[6,0] = theta1_1
    theta[6,1] = theta2_2
    theta[6,2] = theta3_2

    # 4
    theta[3,0] = theta1_2
    theta[3,1] = -theta2_2
    theta[3,2] = -theta3_2

    theta[7,0] = theta1_2
    theta[7,1] = -theta2_2
    theta[7,2] = -theta3_2

    # theta4,5,6******************

    # solve last 3 axes by front 3 axes
    for j in range(0,4):
        th = np.zeros((3,1),np.float)
      
        th = theta[j,0:3] 
           
        R00_4_inv = MDH_TF_f.R_fram_0_4_inv(th[0],th[1],th[2])
        
        R4_6 = np.dot(R00_4_inv, R00_6)  # R4_6=(R00_4^-1)*R00_6
        R4_6_33 = R4_6[2,2]

        if (abs(R4_6_33-1)>0.0001) and ( abs(R4_6_33+1)>0.0001): # !=1 and !=-1
            beta = math.atan2(math.sqrt(R4_6[2,0]*R4_6[2,0] + R4_6[2,1]*R4_6[2,1]), R4_6_33)
            alfa = math.atan2(R4_6[1,2]/math.sin(beta), R4_6[0,2]/math.sin(beta))
            gama = math.atan2(R4_6[2,1]/math.sin(beta), -R4_6[2,0]/math.sin(beta))
        elif abs(R4_6_33-1)<=0.0001: # =1
            alfa = 0
            beta = 0
            gama = math.atan2(-R4_6[0,1],R4_6[0,0])
        elif abs(R4_6_33+1)<=0.0001: # =-1
            alfa = 0
            beta = pi
            gama = math.atan2(R4_6[0,1],-R4_6[0,0])
            pass

        # 1 solution
        if alfa<0:
            theta4_1 = alfa+pi
        else:
            theta4_1 = alfa-pi
            pass
        
        theta5_1 = beta+pi/2 # add axis 5 default extra offset
        theta6_1 = gama 

        # 2 solution
        theta5_2 = -beta+pi/2

        if theta4_1>=0:
            theta4_2 = theta4_1-pi
        else:
            theta4_2 = theta4_1+pi   
            pass

        if theta6_1>=0:
            theta6_2 = theta6_1-pi
        else:
            theta6_2 = theta6_1+pi   
            pass
        
        # each set of solution for the front 3 axes 
        # corresponds to 2 sets of solution for the last 3 axes
        theta[j,3] = theta4_1
        theta[j,4] = theta5_1
        theta[j,5] = theta6_1
        
        theta[j+4,3] = theta4_2
        theta[j+4,4] = theta5_2
        theta[j+4,5] = theta6_2

        if (pi-abs(theta6_1)) <= 0.001: # theta6_1= +-180
            extraSolution = extraSolution+1

            theta16[7+extraSolution,0] = theta[j,0]
            theta16[7+extraSolution,1] = theta[j,1]
            theta16[7+extraSolution,2] = theta[j,2]
            theta16[7+extraSolution,3] = theta[j,3]
            theta16[7+extraSolution,4] = theta[j,4]
            theta16[7+extraSolution,5] = -theta6_1
        elif abs(theta6_1) <= 0.001: # theta6_1=0 theat6_2= +-180
            extraSolution = extraSolution+1

            theta16[7+extraSolution,0] = theta[j+4,0]
            theta16[7+extraSolution,1] = theta[j+4,1]
            theta16[7+extraSolution,2] = theta[j+4,2]
            theta16[7+extraSolution,3] = theta[j+4,3]
            theta16[7+extraSolution,4] = theta[j+4,4]
            theta16[7+extraSolution,5] = -theta6_2
            
            pass

        pass # end of for j in range(0,4):
    theta16[0:8,:] = theta[0:8,:]
    if extraSolution < 8:
        theta16 = np.delete(theta16,obj=np.s_[8+extraSolution:16], axis=0) 
        pass
    print(np.around(theta16*180/pi,3))
    return theta16
    pass

def ti5_optIK(cJdeg, Targ):
    # type:(np.ndarray,np.ndarray)->np.ndarray
    """input:cJdeg/deg, Targ/mm
    ouput:jointIK/deg"""
    try:
        theta = ti5_ik(cJdeg, Targ)
    except Exception as e:
        print('error occurred')
        print(e)
        theta = np.array(cJdeg, np.float)

    numOfSolution = np.size(theta)/6
    theta = theta*(180/pi)
    
    wct = [4,12,6,2,2,1]
    if(0 < numOfSolution): 
        while(numOfSolution > 1):
            minusTheta = abs(theta - cJdeg)
           
            timesTheta = minusTheta * wct
          
            sumTheta = np.sum(timesTheta,axis=1)
           
            minJindex = np.argmin(sumTheta)
            
            if abs(theta[minJindex,0])-170<=0 and abs(theta[minJindex,3])-170<=0 and abs(theta[minJindex,1])-90<=0 \
            and abs(theta[minJindex,2])-150<=0 and theta[minJindex,4]-200<=0 and theta[minJindex,4]+50>=0 \
            and abs(theta[minJindex,5])-180<=0 :
                bestR = theta[minJindex,:]
                return bestR
                break
            else:
                theta = np.delete(theta,obj=minJindex, axis=0) 
                numOfSolution = numOfSolution-1
            pass # while(numOfSolution > 1)
       
        if(1 == numOfSolution):
           
            theta=np.ravel(theta)
            if abs(theta[0])-170<=0 and abs(theta[3])-170<=0 and abs(theta[1])-90<=0 \
            and abs(theta[2])-150<=0 and theta[4]-200<=0 and theta[4]+50>=0 \
            and abs(theta[5])-180<=0 :
                bestR = theta
            else:
                return cJdeg  
    else:
        return cJdeg
        pass
    
    return bestR

    pass

def ti5_optIK_print(cJdeg, Targ):
    # type:(np.ndarray,np.ndarray)->np.ndarray
    try:
        theta = ti5_ik_print(cJdeg, Targ)
    except Exception as e:
        print('error occurred')
        print(e)
        theta = np.array(cJdeg, np.float)

    numOfSolution = np.size(theta)/6
    theta = theta*(180/pi)
    
    wct = [4,12,6,2,2,1]
    if(0 < numOfSolution): 
        while(numOfSolution > 1):
            minusTheta = abs(theta - cJdeg)
           
            timesTheta = minusTheta * wct
          
            sumTheta = np.sum(timesTheta,axis=1)
           
            minJindex = np.argmin(sumTheta)
            
            if abs(theta[minJindex,0])-170<=0 and abs(theta[minJindex,3])-170<=0 and abs(theta[minJindex,1])-90<=0 \
            and abs(theta[minJindex,2])-150<=0 and theta[minJindex,4]-200<=0 and theta[minJindex,4]+50>=0 \
            and abs(theta[minJindex,5])-180<=0 :
                bestR = theta[minJindex,:]
                
                break
            else:
                theta = np.delete(theta,obj=minJindex, axis=0) 
                numOfSolution = numOfSolution-1
            pass # while(numOfSolution > 1)
        
        if(1 == numOfSolution):
            if abs(theta[0])-170<=0 and abs(theta[3])-170<=0 and abs(theta[1])-90<=0 \
            and abs(theta[2])-150<=0 and theta[4]-200<=0 and theta[4]+50>=0 \
            and abs(theta[5])-180<=0 :
                bestR = theta
            else:
                return cJdeg  
    else:
        return cJdeg
        pass
    
    return bestR

    pass


def TFMeef(xyz_eulZYX):
    # type:(np.ndarray)->np.ndarray
    """input xyz unit is mm angle unit is rad"""
    alfa = xyz_eulZYX[3]
    beta = xyz_eulZYX[4]
    gama = xyz_eulZYX[5]

    # 注意：此处T是浅拷贝，会直接改变xyz_eulZYX的值
    T = np.eye(4,dtype= np.float)
    T[0:3,3] = xyz_eulZYX[0:3]

    T[0,0] = math.cos(alfa) * math.cos(beta) 
    T[0,1] = math.cos(alfa) * math.sin(beta) * math.sin(gama) - math.sin(alfa) * math.cos(gama) 
    T[0,2] = math.cos(alfa) * math.sin(beta) * math.cos(gama) + math.sin(alfa) * math.sin(gama)
    T[1,0] = math.sin(alfa) * math.cos(beta)
    T[1,1] = math.sin(alfa) * math.sin(beta) * math.sin(gama) + math.cos(alfa) * math.cos(gama) 
    T[1,2] = math.sin(alfa) * math.sin(beta) * math.cos(gama) - math.cos(alfa) * math.sin(gama)
    T[2,0] = -math.sin(beta)
    T[2,1] = math.cos(beta) * math.sin(gama)
    T[2,2] = math.cos(beta) * math.cos(gama)
    
    return T
    pass

def mdhTfM(alfa_i_1, a_i_1, theta_i, d_i):
    # type:(float, float, float, float)->np.ndarray
    """input angle unit is rad"""
    T = np.eye(4, dtype=np.float)
    T[0,0] = math.cos(theta_i)
    T[0,1] = -math.sin(theta_i)
    T[0,2] = 0
    T[0,3] = a_i_1
    T[1,0] = math.sin(theta_i)*math.cos(alfa_i_1)
    T[1,1] = math.cos(theta_i)*math.cos(alfa_i_1)
    T[1,2] = -math.sin(alfa_i_1)
    T[1,3] = -math.sin(alfa_i_1)*d_i
    T[2,0] = math.sin(theta_i)*math.sin(alfa_i_1)
    T[2,1] = math.cos(theta_i)*math.sin(alfa_i_1)
    T[2,2] = math.cos(alfa_i_1)
    T[2,3] = math.cos(alfa_i_1)*d_i
 
    return T
    pass

def ti5FK(theta):
    # type:(np.ndarray)->np.ndarray
    """input angle unit is rad"""
    T01 = mdhTfM(0.0,     0.0,   0+theta[0],     190.0)
    T12 = mdhTfM(pi/2,    0.0,   pi/2+theta[1],  0.0  )
    T23 = mdhTfM(0.0,     400.0, -pi/2+theta[2], 0.0  )
    T34 = mdhTfM(-pi/2,   0.0,   0+theta[3],     300.0)
    T45 = mdhTfM(pi/2,    0.0,   pi/2+theta[4],  0.0  )
    T56 = mdhTfM(pi/2,    0.0,   0+theta[5],     300.0)
    Tf = np.dot(np.dot(np.dot(np.dot(np.dot(T01, T12), T23), T34), T45), T56)

    return Tf
    pass

def rotM2Euler_ZYX(r):
    # type:(np.ndarray)->np.ndarray
    """" input:rotation matrix
         output:Euler_ZYX/deg  """
    if(abs(r[2,0]-1)<=0.000001): # 1
        alfa = 0
        alfa2 = 0
        beta = -pi/2
        beta2 = -pi/2
        gama = math.atan2(-r[0,1], -r[2,0])# gama-alfa  if alfa=0 then this is gama
        gama2 = gama
    elif(abs(r[2,0]+1)<=0.000001):# -1
        alfa = 0
        alfa2 = 0
        beta = pi/2
        beta2 = pi/2
        gama = math.atan2(r[0,1], r[2,0])# gama-alfa  if alfa=0 then this is gama
        gama2 = gama
    else:
        beta = math.asin(-r[2,0])
        beta2 = pi - beta
        alfa = math.atan2(r[1,0]/math.cos(beta), r[0,0]/math.cos(beta))
        alfa2 = math.atan2(r[1,0]/math.cos(beta2), r[0,0]/math.cos(beta2))
        gama = math.atan2(r[2,1]/math.cos(beta), r[2,2]/math.cos(beta))
        gama2 = math.atan2(r[2,1]/math.cos(beta2), r[2,2]/math.cos(beta2))
        pass
    

    return np.array([alfa*180/pi, beta*180/pi ,gama*180/pi])
    pass


def ti5SimpleIK_p2j(cJdeg,tPose):
    """input:cJdeg/deg, tPose/mm_deg
    """
    tPose[3] = tPose[3]*pi/180
    tPose[4] = tPose[4]*pi/180
    tPose[5] = tPose[5]*pi/180
    return ti5_optIK(cJdeg,Targ=TFMeef(tPose))

    pass

def ti5SimpleIK_t2j(cJdeg,Targ):
    
    return ti5_optIK(cJdeg,Targ)
    pass

def ti5_j2p(cJdeg):
    # type:(np.ndarray)->np.ndarray
    """ cJdeg to xyz_eulZYX/mm_deg
    input:cJdeg/deg
    otput:xyz_eulZYX/mm_deg """
    T = ti5FK(cJdeg*pi/180)
    x = T[0,3]
    y = T[1,3]
    z = T[2,3]
    eulZYX = np.array(rotM2Euler_ZYX(T[0:3,0:3]))

    return np.array([x,y,z,eulZYX[0],eulZYX[1],eulZYX[2]])
    pass



# def testMotor_LinerPoly(targetJDeg, pub,cmd_rate,use_time,flag):
#     # 如果需要自动运行令auto_run=1
#     auto_run = 1 # 
    
#     jointTarget = ti5_polyJC_Lists()
#     # **************************config ctrl mode********************************
#     jointTarget.numOfCmd = 6
#     jointTarget.interpolyReq = 10 # iner linear interpoly
#     jointTarget.mode = 0
#     jointTarget.header.stamp = rospy.Time().now()
#     jointTarget.cmdRate = int(cmd_rate)
#     # **************************************************************************
#     print("*********+++++LinerPoly start+++*********")
    
#     # from pose 0 to pose 1
#     if auto_run==1:
#         #jointxRadList is deg
#         if flag==1:
#             nJ = [  0.0,    0.0,   -90,     0.0,    0.0,   0.0,   0.0,
#                 targetJDeg[0],targetJDeg[1],targetJDeg[2],targetJDeg[3],targetJDeg[4],targetJDeg[5],-600000]
#         elif flag==2:
#             nJ = [ targetJDeg[0],targetJDeg[1],targetJDeg[2],targetJDeg[3],targetJDeg[4],targetJDeg[5],-600000 ,
#                   0.0,    0.0,   -90,     0.0,    0.0,   0.0,  0.0
#                 ]
#         for i in range(0,len(nJ)/7):
#             jointTarget.joint1RadList.append(nJ[0+i*7])
#             jointTarget.joint2RadList.append(nJ[1+i*7])
#             jointTarget.joint3RadList.append(nJ[2+i*7])
#             jointTarget.joint4RadList.append(nJ[3+i*7])
#             jointTarget.joint5RadList.append(nJ[4+i*7])
#             jointTarget.joint6RadList.append(nJ[5+i*7])
#             jointTarget.joint7RadList.append(nJ[6+i*7])

#         jointTarget.timeLists = [0, use_time]
    
#         pub.publish(jointTarget)
#         print("LinerPoly pub ok")
   
#     pass


# *************************************************************

if  __name__== "__main__":
    # rospy.init_node("ti5IK_node")
    rospy.init_node("ti5_fkik_node")
 


    # Targ = np.array([[0,1,0,0],
    #                  [1,0,0,-300],
    #                  [0,0,-1,290],
    #                  [0,0,0,1]],dtype= np.float)
   

    # print(rospy.Time.now().to_nsec())
    # time1=rospy.Time.now().to_nsec()


    # jd = ti5_optIK(cJdeg=np.array([-90,0,-90,0,0,-90],np.float),Targ=Targ)
    # print(np.around(jd,3))
    # T = TFMeef(np.array([0,-300,290,90*pi/180, 0, 180*pi/180],np.float))
    # print(T)
    # jd = ti5_optIK(cJdeg=np.zeros((1,6),np.float),Targ=T)
    # print('opjd',np.around(jd,3))

    # eul_ZYX = rotM2Euler_ZYX(T[0:3,0:3])
    # print('eul_ZYX',eul_ZYX)
    # theta = np.array([10.0000, 20.0000, 30.0000, 40.00, 50.0000, 60.0000])*pi/180
    # T = ti5FK(theta)
    # print(T)
    # jd = ti5_optIK_print(cJdeg=np.zeros((1,6),np.float),Targ=T)
    # print('opjd',np.around(jd,3))

    jd = ti5SimpleIK_p2j(cJdeg=np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0], np.float),
                        tPose=np.array([60.0, -480.0, 60.0, 90.0, 0.0, 180.0], np.float))
    print('jd',jd)



    # print(rospy.Time.now().to_nsec())
    # time2 = rospy.Time.now().to_nsec()
    # print((time2-time1)/1000000)

    # sPose = ti5_j2p(np.array([0,0,0,0,0,0]))
    # ePose = ti5_j2p(np.array([0,0,-90,0,0,0]))
    # print('sPose', sPose )
    # print('ePose', ePose )

    # from interPolyPointPub import move_p
    # from serial_demo.msg import ti5_polyJC_Lists
    # pub = rospy.Publisher("interPolyPoint_topic",ti5_polyJC_Lists,queue_size=1000)
    # move_p(sPose, ePose, pub=pub, cmd_rate=500, use_time=6,cJdeg=np.array([0,0,0,0,0,0]))




